/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-03-07     Hao       the first version
 */
#include "inline_current_sense.h"
#include "time_utils.h"
#include "default.h"

#include <hardware/adc_operate.h>

InlineCurrentSense::InlineCurrentSense(float shunt_resistor, float gain, uint8_t chA, uint8_t chB, uint8_t chC)
{
    ch_a_ = chA;
    ch_b_ = chB;
    ch_c_ = chC;

    /*
     对于电流传感器：
     Vout = Vdiff * gain + Vref
     因此：
     Vdiff = (Vout - Vref) / gain
     电流：
     I = Vdiff / _shunt_resistor = (Vout - Vref) / gain / _shunt_resistor
     提取：
     volts_to_amps_ratio = 1 / gain / _shunt_resistor
     电流计算时：
     I = (Vout - Vref) * volts_to_amps_ratio
     */
    shunt_resistor_ = shunt_resistor;

    volts_to_amps_ratio_ = 1.0f / shunt_resistor_ / gain; // volts to amps

    // gains for each phase
    gain_a_ = volts_to_amps_ratio_;
    gain_b_ = volts_to_amps_ratio_;
    gain_c_ = volts_to_amps_ratio_;
}
;

InlineCurrentSense::~InlineCurrentSense()
{

}

// Inline sensor init function
int InlineCurrentSense::init()
{
    SIMPLE_DEBUG("InlineCurrentSense init \n");

    delayMicroseconds(100);

    // 校准ADC零电流偏差
    calibrateOffsets();

    return 1;
}

// 检测0电流时ADC偏移量
void InlineCurrentSense::calibrateOffsets()
{
    const int calibration_rounds = 1000;

    // find adc offset = zero current voltage
    offset_ia_ = 0;
    offset_ib_ = 0;
    offset_ic_ = 0;

    // read the adc voltage 1000 times ( arbitrary number )
    for (int i = 0; i < calibration_rounds; i++)
    {
        if (_isset(ch_a_))
            offset_ia_ += readADCVoltageInline(ch_a_);
        if (_isset(ch_b_))
            offset_ib_ += readADCVoltageInline(ch_b_);
        if (_isset(ch_c_))
            offset_ic_ += readADCVoltageInline(ch_c_);
        delayMillisecond(1);
    }
    // calculate the mean offsets
    if (_isset(ch_a_))
        offset_ia_ = offset_ia_ / calibration_rounds;
    if (_isset(ch_b_))
        offset_ib_ = offset_ib_ / calibration_rounds;
    if (_isset(ch_c_))
        offset_ic_ = offset_ic_ / calibration_rounds;

    SIMPLE_DEBUG("offset_ia_ = %d.%d \n", (int)(offset_ia_ * 1000) / 1000, (int)(offset_ia_ * 1000) % 1000);
    SIMPLE_DEBUG("offset_ib_ = %d.%d \n", (int)(offset_ib_ * 1000) / 1000, (int)(offset_ib_ * 1000) % 1000);
    SIMPLE_DEBUG("offset_ic_ = %d.%d \n", (int)(offset_ic_ * 1000) / 1000, (int)(offset_ic_ * 1000) % 1000);
}

// 获取三项电流，两项或者三项
PhaseCurrent_s InlineCurrentSense::getPhaseCurrents()
{
    PhaseCurrent_s current;
    current.a = _isset(ch_a_) ? (readADCVoltageInline(ch_a_) - offset_ia_) * gain_a_ : 0;
    current.b = _isset(ch_b_) ? (readADCVoltageInline(ch_b_) - offset_ib_) * gain_b_ : 0;
    current.c = _isset(ch_c_) ? (readADCVoltageInline(ch_c_) - offset_ic_) * gain_c_ : 0;

    return current;
}

// get current magnitude
//   - absolute  - if no electrical_angle provided
//   - signed    - if angle provided
float InlineCurrentSense::getDCCurrent(float motor_electrical_angle)
{
    // read current phase currents
    PhaseCurrent_s current = getPhaseCurrents();
    // currnet sign - if motor angle not provided the magnitude is always positive
    float sign = 1;

    // calculate clarke transform
    float i_alpha, i_beta;
    if (!current.c)
    {
        // if only two measured currents
        i_alpha = current.a;
        i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
    }
    if (!current.a)
    {
        // if only two measured currents
        float a = -current.c - current.b;
        i_alpha = a;
        i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b;
    }
    if (!current.b)
    {
        // if only two measured currents
        float b = -current.a - current.c;
        i_alpha = current.a;
        i_beta = _1_SQRT3 * current.a + _2_SQRT3 * b;
    }
    else
    {
        // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed.
        float mid = (1.f / 3) * (current.a + current.b + current.c);
        float a = current.a - mid;
        float b = current.b - mid;
        i_alpha = a;
        i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
    }

    // if motor angle provided function returns signed value of the current
    // determine the sign of the current
    // sign(atan2(current.q, current.d)) is the same as c.q > 0 ? 1 : -1
    if (motor_electrical_angle)
    {
        float ct;
        float st;
        _sincos(motor_electrical_angle, &st, &ct);
        sign = (i_beta * ct - i_alpha * st) > 0 ? 1 : -1;
    }
    // return current magnitude
    return sign * _sqrtApprox(i_alpha * i_alpha + i_beta * i_beta);
}

// function used with the foc algorihtm
//   calculating DQ currents from phase currents
//   - function calculating park and clarke transform of the phase currents
//   - using getPhaseCurrents internally
DQCurrent_s InlineCurrentSense::getFOCCurrents(float angle_el)
{
    // read current phase currents
    PhaseCurrent_s current = getPhaseCurrents();

    // calculate clarke transform
    float i_alpha, i_beta;
    if (!current.c)
    {
        // if only two measured currents
        i_alpha = current.a;
        i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
    }
    if (!current.a)
    {
        // if only two measured currents
        float a = -current.c - current.b;
        i_alpha = a;
        i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b;
    }
    if (!current.b)
    {
        // if only two measured currents
        float b = -current.a - current.c;
        i_alpha = current.a;
        i_beta = _1_SQRT3 * current.a + _2_SQRT3 * b;
    }
    else
    {
        // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed.
        float mid = (1.f / 3) * (current.a + current.b + current.c);
        float a = current.a - mid;
        float b = current.b - mid;
        i_alpha = a;
        i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
    }

    // calculate park transform
    float ct;
    float st;
    _sincos(angle_el, &st, &ct);
    DQCurrent_s return_current;
    return_current.d = i_alpha * ct + i_beta * st;
    return_current.q = i_beta * ct - i_alpha * st;
    return return_current;
}

float InlineCurrentSense::readADCVoltageInline(uint8_t phase)
{
    return adc_getChVoltage(phase);
}

